//
// Created by hoshino on 2021/3/27.
//

#ifndef RADAR_3DPNP_H
#define RADAR_3DPNP_H
#include <opencv2/opencv.hpp>
#include <vector>


typedef struct
{
    double yaw = 0;//5
    double roll = 0;
    double pitch = 0;//-15
}Radar_angle;

class radar_pnp{
public:
    bool status = false;
    bool DEBUG = false;
    const int armor_w = 0;
    const int armor_h = 0;
    const int arena_l= 28000;  //长 28m
    const int arena_w = 150000;  //宽 15m
    cv::Point3d radar_coordinate_around = {0.0,0.0,0.0};//
    cv::Point3d radar_coordinate = {0.0,0.0,0.0};//
    Radar_angle radar_angle_around;
    Radar_angle radar_angle;
    cv::Mat mtx,dist;
    bool radar_coordinate_init(std::vector<std::vector<cv::Point3f>> &visual_coordinate,std::vector<std::vector<cv::Point3f>> &visual_coordinate_camara);
    bool radar_debug_init();
    std::vector<cv::Point2d> radar_solvePnp(std::vector<std::vector<cv::Point2d>> &aim_coordinate_camara) const;
    cv::Point2d radar_rotate(cv::Mat &Tvec,double thata_y) const;
    bool coordinate_judge(cv::Mat &Tvec,double angle_x,double angle_y,double angle_z) const;
};




#endif //RADAR_3DPNP_H
